/**
 * @file Quaternion.cpp
 * @author  Aljosa Osep <aljosa.osep@gmail.com, Fletcher Dunn, Ian Parnberry>
 * @version 1.0
 *
 * @section LICENSE
 *
 * Redistribution and use in source and binary forms, with or without modification, are
 * permitted provided that the following conditions are met:

 *  1. Redistributions of source code must retain the above copyright notice, this list of
 *    conditions and the following disclaimer.

 *  2. Redistributions in binary form must reproduce the above copyright notice, this list
 *     of conditions and the following disclaimer in the documentation and/or other materials
 *     provided with the distribution.

 * THIS SOFTWARE IS PROVIDED BY <Aljosa Osep> ``AS IS'' AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL <Aljosa Osep>> OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 * The views and conclusions contained in the software and documentation are those of the
 * authors and should not be interpreted as representing official policies, either expressed
 * or implied, of <Aljosa Osep>.
 *
 * @section DESCRIPTION
 *
 * Quaternion - class representing an quaternion
 * Based on examples from 3D Math Premier of Graphics and Games Development book
 * Original authors: Fletcher Dunn, Ian Parnberry. Thanks!
 */

#include "Quaternion.h"
#include "MathUtil.h"
#include "Vector3.h"
#include "EulerAngles.h"


namespace xEngine
{
    namespace PlatformIndependent
    {
        namespace Math
        {

            /// Operations
            /**
             * Quaternion identity
             *
             * Sets quaterion to identity quaternion
             *
             */
            void Quaternion::identity() {
                this->w = 1.0f;
                this->x = this->y = this->z = 0.0f;
            }

            // Set quaternion to a specific rotation
            /**
             * Rotate about x-axis
             *
             * Sets quaternion to rotate about x-axis
             *
             * @param  theta ... angle
             */
            void Quaternion::setToRotateAboutX(float theta) {
                // half angle
                float halfTheta = theta * 0.5f;

                // set values
                w = cos(halfTheta);
                x = sin(halfTheta);
                y = 0.0f;
                z = 0.0f;
            }

            /**
             * Rotate about y-axis
             *
             * Sets quaternion to rotate about y-axis
             *
             * @param  theta ... angle
             */
            void Quaternion::setToRotateAboutY(float theta) {
                // half angle
                float halfTheta = theta * 0.5f;

                // set values
                w = cos(halfTheta);
                x = 0.0f;
                y = sin(halfTheta);
                z = 0.0f;
            }

            /**
             * Rotate about z-axis
             *
             * Sets quaternion to rotate about z-axis
             *
             * @param  theta ... angle
             */
            void Quaternion::setToRotateAboutZ(float theta) {
                // half angle
                float halfTheta = theta * 0.5f;

                // set values
                w = cos(halfTheta);
                x = 0.0f;
                y = 0.0f;
                z = sin(halfTheta);
            }

            /**
             * Rotate about arbitrary axis
             *
             * Sets quaternion to rotate about z-axis
             *
             * @param  theta ... angle, axis ... axis of rotation, (normalised) Vector3
             */
            void Quaternion::setToRotateAboutAxis(const Vector3 &axis, float theta) {
               // if (axis.length() > 1.05)
               //     axis.normalize(); // Axis must be normalised

                // Axis is normalised
                float halfTheta = theta * 0.5f;
                float sinHalfTheta = sin (halfTheta);

                // Set the quaternion values
                w = cos(halfTheta);
                x = axis.x * sinHalfTheta;
                y = axis.y * sinHalfTheta;
                z = axis.z * sinHalfTheta;
            }


            // Setup to pefrom object <-> inertial rotations, in Euler format

            /**
             * Rotate perform rotation from object space to inertial (axis-aligned) space
             *
             * Setup to pefrom object <-> inertial rotations
             *
             * @param  orientation (EulerAngles)
             * @return Quaternion based on that orientation
             */
            void Quaternion::setToRotateObjectToInertial(const EulerAngles &orientation) {
                // Variables
                float sp, sb, sh;
                float cp, cb, ch;

                // Get sine and cosine values for pitch, bank and heading
                sinCos(&sp, &cp, orientation.pitch * 0.5f);
                sinCos(&sb, &cb, orientation.bank * 0.5f);
                sinCos(&sh, &ch, orientation.heading * 0.5f);

                // Calculate quaternion values
                w = ch*cp*cb + sh*sp*sb;
                x = ch*sp*cb + sh*cp*sb;
                y = -ch*sp*sb + sh*cp*cb;
                z = -sh*sp*cb + ch*cp*sb;

            }

            /**
             * Rotate perform rotation from inertial(axis-aligned) space to object space
             *
             * Setup to pefrom object <-> inertial rotations
             *
             * @param  orientation (EulerAngles)
             * @return Quaternion based on that orientation
             */
            void Quaternion::setToRotateInertialToObject(const EulerAngles &orientation) {
                // Variables
                float sp, sb, sh;
                float cp, cb, ch;

                // Get sine and cosine values for pitch, bank and heading
                sinCos(&sp, &cp, orientation.pitch * 0.5f);
                sinCos(&sb, &cb, orientation.bank * 0.5f);
                sinCos(&sh, &ch, orientation.heading * 0.5f);

                // Calculate quaternion values
                w = ch*cp*cb + sh*sp*sb;
                x = -ch*sp*cb - sh*cp*sb;
                y = ch*sp*sb - sh*cp*cb;
                z = sh*sp*cb - ch*cp*sb;
            }


            /**
             * Cross product
             *
             * Quaternion cross product - contatenates multiple angular
             * displacements, from left to right.
             * Warning! this is backward from standard definition.
             *
             * @param  Quaternion
             * @return Quaternion
             */
            Quaternion Quaternion::operator *(const Quaternion &a) const {
                Quaternion r; // Result quaternion

                r.w = w*a.w - x*a.x - y*a.y - z*a.z;
                r.x = w*a.x + x*a.w + z*a.y - y*a.z;
                r.y = w*a.y + y*a.w + x*a.z - z*a.x;
                r.z = w*a.z + z*a.w + y*a.x - x*a.y;

                return r;
            }

            /**
             * Cross product and assignment combined
             *
             * Quaternion cross product - contatenates multiple angular
             * displacements, from left to right.
             * Warning! this is backward from standard definition.
             *
             * @param  Quaternion
             * @return Quaternion
             */
            Quaternion &Quaternion::operator *=(const Quaternion &a) {
                *this = *this * a;

                return *this;
            }


            /**
             * Normalize quaternion
             *
             * Normalize a quaternion. Provided to combat float error.
             *
             */
            void Quaternion::normalize() {
                // Compute the magnitude
                float mag = (float)sqrt(w*w + x*x + y*y + z*z);

                if (mag > 0.0f) {
                    float oneOverMag = 1.0f / mag;
                    w *= oneOverMag;
                    x *= oneOverMag;
                    y *= oneOverMag;
                    z *= oneOverMag;
                }
                else
                {
                    // There seems to be a problem.
                    assert(false);

                    // In a release, slam it to identity
                    identity();
                }
            }


            /**
             * Gets angle of rotation
             *
             * Extracts and return orientation angle
             *
             * @return angle
             */
            float Quaternion::getRotationAngle() const {
                return safeAcos(w) * 2.0f;
            }

            /**
             * Gets axis of rotation
             *
             * Extracts and return orientation axis
             *
             * @return angle
             */
            Vector3 Quaternion::getRotationAxis() const {
                // TODO
                // ?
            }



            /// Nonmembers
            /**
             * Dot product
             *
             * Quaternion dot product ;)
             *
             * @param  Quaternion b
             * @param  Quaternion a
             * @return float value (result of a*b)
             */
            extern float dotProduct(const Quaternion &a, const Quaternion &b) { // Dot product
                return a.w*b.w + a.x*b.x + a.y*b.y + a.z * b.z;
            }

            /**
             * SLERP
             *
             * Spherical linear interpolation
             *
             * @param  Quaternion q0 (orgin orientation)
             * @param  Quaternion q1 (destination orientation)
             * @param  t ... amount of interpolation
             * @return float value (result of a*b)
             */
            extern Quaternion slerp(const Quaternion &q0, const Quaternion &q1, float t) { // Spherical line interpolation
                // Check parameter t borders
                if (t <= 0.0f) return q0;
                if (t >= 1.0f) return q1;

                // Calculate cosine of angle between quaterions
                float cosOmega = dotProduct(q0, q1);

                // If negative dot, negate one of input
                // quaterions, to take the shorter 4D arc
                float q1w = q1.w;
                float q1x = q1.x;
                float q1y = q1.y;
                float q1z = q1.z;


                if (cosOmega < 0.0f) {
                    q1w = -q1w;
                    q1x = -q1x;
                    q1y = -q1y;
                    q1z = -q1z;

                    cosOmega = -cosOmega;
                }

                // We should have two unit Q's => dot should be <= 1.0f
                assert(cosOmega < 1.1);

                // Compute interpolation fraction
                float k0, k1;
                if (cosOmega > 0.9999f) { // Q's nearly the same
                    // Linear interpolation
                    k0 = 1.0f - t;
                    k1 = t;
                }
                else
                {
                    // Calculate sin of omega
                    float sinOmega = sqrt(1.0f - cosOmega*cosOmega);

                    // Compute angle
                    float omega = atan2(sinOmega, cosOmega);

                    // Inverse of denominator
                    float oneOverSinOmega = 1.0f / sinOmega;

                    // Compute interpolation params
                    k0 = sin((1.0f -t ) * omega) * oneOverSinOmega;
                    k1 = sin(t * omega) * oneOverSinOmega;
                }

                // Interpolate
                Quaternion r;

                r.x = k0*q0.x + k1*q1.x;
                r.y = k0*q0.y + k1*q1.y;
                r.z = k0*q0.x + k1*q1.z;

                return r;
            }

            /**
             * Conjugate
             *
             * Returns an quaternion with opposite orientation
             *
             * @param  Quaternion
             * @return Quaternion's conjugate (Quaternion with opposite orientation)
             */
            extern Quaternion conjugate(const Quaternion &q) { // Conjugate
                Quaternion r;

                r.w = q.w; // Same rotation amount

                r.x = -q.x;
                r.y = -q.y;
                r.z = -q.z;

                return r;
            }

            /**
             * Power
             *
             * Returns an quaternion's exponentiation.
             *
             * @param  Quaternion
             * @param exponent (float)
             * @return Quaternion's conjugate (Quaternion with opposite orientation)
             */
            extern Quaternion pow(const Quaternion &q, float exponent) {
                // Check for the case of an identity quaternion
                if (fabs(q.w) > 0.9999f)
                    return q;

                // Extract half alpha angle
                float alpha = acos(q.w);

                // Calc. new alpha
                float newAlpha = alpha * exponent;

                // Compute Q. values
                Quaternion r;
                r.w = cos(newAlpha);

                float mult = sin(newAlpha) / sin(alpha);
                r.x = q.x * mult;
                r.y = q.y * mult;
                r.z = q.z * mult;

                return r;
            }

        }
    }
}
